# Untitled - By: 帅天帝 - 周一 9月 1 2025

import network, time, sensor, image, lcd, math, json, _thread, utime
from machine import UART,Timer
from maix import GPIO
from fpioa_manager import fm
from simple import MQTTClient

SSID='Redmi' # WiFi 账号
PWD='12345678'  # WiFi 密码

# 颜色识别阈值 (L Min, L Max, A Min, A Max, B Min, B Max) LAB模型
thresholds = [
    (2, 55, 2, 34, 18, 36), # 棕色快递箱阈值
    (15, 65, 23, 1, -13, 13),  #粉色
    (3, 50, 36, 76, 64, 23), # 红色阈值
    (0, 60, -10, -62, 54, 10), # 绿色阈值
    (0, 30, 0, 64, -128, -20) # 蓝色定位点阈值
    ]

#三个定位点坐标
anchor_points = []
#成功找到三个定位点的标志
anchor_point_flag = 0
#快递箱识别中心点坐标
original_center_point = None
#成功找到快递箱识别中心点坐标的标志
original_center_point_flag = 0
#快递箱相对坐标集，采集5个取平均值
central_coordinates_list = []
#快递箱相对坐标
central_coordinates = None
#快递码正确识别到的标志
tracking_code_ready_flag = 0
#快递码，取5个判断是否误判
tracking_codes = []
#上一个快递码，用于判断是否重复发送
last_tracking_code = 0
#快递种类,放置仓库(颜色) A->绿色普通件 B->红色紧急件 C->EMS专用件
warehouse = 0


#################### GPIO初始化 ########################
#将蓝灯引脚IO35配置到GPIO0，K210引脚支持任意配置
fm.register(35, fm.fpioa.GPIO0)

LED_B = GPIO(GPIO.GPIO0, GPIO.OUT) #构建LED对象

fm.register(16, fm.fpioa.GPIOHS1)

KEY = GPIO(GPIO.GPIOHS1, GPIO.IN, GPIO.PULL_UP) #构建KEY对象

#中断回调函数，用于重新发送一次
def fun(KEY):
    global last_tracking_code
    utime.sleep_ms(10) #消除抖动
    if KEY.value()==0: #确认按键被按下
        last_tracking_code = 0

#开启中断，下降沿触发
KEY.irq(fun, GPIO.IRQ_FALLING)
#######################################################



#################### WiFi模块初始化 ########################
#使能引脚初始化
fm.register(25, fm.fpioa.GPIOHS0, force=True)
wifi_en=GPIO(GPIO.GPIOHS0, GPIO.OUT)

#串口初始化
fm.register(13, fm.fpioa.UART2_TX, force=True)
fm.register(15, fm.fpioa.UART2_RX, force=True)
uart = UART(UART.UART2,115200,timeout=1000,read_buf_len=4096)

#WiFi使能函数，ESP8266-01S不需要使能
def wifi_enable(en):
    global wifi_en
    wifi_en.value(en)

def wifi_init():
    global uart
    #先禁用再启用 WiFi，进行硬件复位，ESP8266-01S要断电复位
    wifi_enable(0)
    time.sleep_ms(200)
    wifi_enable(1)
    time.sleep(2)
    #初始化 UART2 并配置为 115200 波特率
    uart = UART(UART.UART2,115200,timeout=1000, read_buf_len=4096)
    tmp = uart.read()
    #发送 AT 指令修改模块波特率为 921600（提高传输速度）
    uart.write("AT+UART_CUR=921600,8,1,0,0\r\n")
    print(uart.read())
    #重新初始化 UART2 为 921600 波特率，使用更大的接收缓冲区
    uart = UART(UART.UART2,921600,timeout=1000, read_buf_len=10240) # important! baudrate too low or read_buf_len too small will loose data
    #发送 AT 指令检查模块是否正常响应
    uart.write("AT\r\n")
    time.sleep_ms(200)
    tmp = uart.read()
    print(tmp)
    if not tmp.endswith("OK\r\n"):
        print("reset fail")
        return None
    try:
        #如果响应正常，创建 ESP8285 网络接口对象并返回 (ESP8285和ESP8266兼容)
        nic = network.ESP8285(uart)
    except Exception:
        return None
    return nic

#构建WiFi对象并使能
wlan = wifi_init()

#正在连接印提示
print("Trying to connect... (may take a while)...")

#连接网络
wlan.connect(SSID,PWD)

#打印IP相关信息
print(wlan.ifconfig())
###################################################



################## MQTT初始化 ######################
#使用thingscloud云平台
SERVER = 'sh-1-mqtt.iot-api.com'
PORT = 1883
CLIENT_ID = 'CanMV-K210' # 客户端ID
TOPIC = 'attributes' # TOPIC名称
USER = 'brqqr11kdowdp945'
PSW = 'WlikZCzmpM'
#创建MQTT对象
client = MQTTClient(CLIENT_ID, SERVER, PORT, USER, PSW, keepalive=60)
#连接MQTT服务器
client.connect()

#网络初始化成功点亮一个LED,否则断电重启
LED_B.value(0) #点亮LED

#发布数据任务
def MQTT_Send():
    global tracking_codes
    global warehouse
    payload = json.dumps({
        "TrackingCode": "{}".format(tracking_codes[0]),  # 快递码，字符串格式
        "warehouse": "{}".format(warehouse)  #快递种类
    })
    client.publish(TOPIC, payload)
####################################################



################### 串口初始化 #######################
#映射串口引脚
fm.register(6, fm.fpioa.UART1_RX, force=True)
fm.register(7, fm.fpioa.UART1_TX, force=True)

#初始化串口
uart = UART(UART.UART1, 115200, read_buf_len=4096)

####################################################



################## 摄像头初始化 ######################
lcd.init(freq=15000000)             # 初始化LCD
sensor.reset()                      # 复位和初始化摄像头，执行sensor.run(0)停止。
sensor.set_vflip(1)                 # 将摄像头设置成后置方式（所见即所得）
sensor.set_hmirror(1)               # GC0328摄像头（如果使用ov2640摄像头，注释此行。）

sensor.set_pixformat(sensor.RGB565) # 设置像素格式为彩色 RGB565 (或灰色)
sensor.set_framesize(sensor.QVGA)   # 设置帧大小为 QVGA (320x240)
sensor.skip_frames(time = 2000)     # 等待设置生效.
clock = time.clock()                # 创建一个时钟来追踪 FPS（每秒拍摄帧数）
####################################################



################## 中心点计算函数 ######################
def identify_anchors(anchor_points):
    """
    根据屏幕位置特征识别三个锚点
    屏幕左上角为(0,0)，y值向下递增
    返回：(左下锚点, 左上锚点, 右下锚点)
    """
    # 筛选左下和左上锚点（x坐标较小的两个）
    sorted_by_x = sorted(anchor_points, key=lambda p: p[0])
    left_candidates = sorted_by_x[:2]  # x最小的两个点
    right_candidate = sorted_by_x[2]   # x最大的点（右下锚点）

    # 在左侧候选点中，区分左下（y较大）和左上（y较小）
    left_candidates_sorted_by_y = sorted(left_candidates, key=lambda p: p[1], reverse=True)
    bottom_left = left_candidates_sorted_by_y[0]  # y较大的是左下
    top_left = left_candidates_sorted_by_y[1]     # y较小的是左上

    return bottom_left, top_left, right_candidate


def calculate_logical_coordinate(anchor_points, original_center_point):
    """使用固定的锚点映射关系计算逻辑坐标"""
    # 识别并固定三个锚点的角色
    bottom_left, top_left, bottom_right = identify_anchors(anchor_points)

    # 定义映射关系：左下(0,0)，左上(0,100)，右下(100,0)
    origin_screen = bottom_left       # 左下 -> (0, 0)
    y_axis_screen = top_left          # 左上 -> (0, 100)
    x_axis_screen = bottom_right      # 右下 -> (100, 0)

    # 提取坐标值
    ox, oy = origin_screen
    x1, y1 = y_axis_screen
    x2, y2 = x_axis_screen

    # 计算目标点相对于原点的偏移
    tx = original_center_point[0] - ox
    ty = original_center_point[1] - oy

    # 计算Y轴向量和X轴向量
    y_vec_x = x1 - ox  # 左上 - 左下（Y轴方向）
    y_vec_y = y1 - oy
    x_vec_x = x2 - ox  # 右下 - 左下（X轴方向）
    x_vec_y = y2 - oy

    # 计算目标点在Y轴和X轴上的投影比例
    # 使用点积计算投影长度，避免除零错误
    y_vec_len_sq = y_vec_x**2 + y_vec_y**2
    x_vec_len_sq = x_vec_x**2 + x_vec_y**2

    y_proj = (tx * y_vec_x + ty * y_vec_y) / y_vec_len_sq if y_vec_len_sq != 0 else 0
    x_proj = (tx * x_vec_x + ty * x_vec_y) / x_vec_len_sq if x_vec_len_sq != 0 else 0

    # 计算逻辑坐标
    logical_x = x_proj * 100
    logical_y = y_proj * 100

    return (logical_x, logical_y)
####################################################


def barcode_name(code): #条形码类型
    if(code.type() == image.EAN2):
        return "EAN2"
    if(code.type() == image.EAN5):
        return "EAN5"
    if(code.type() == image.EAN8):
        return "EAN8"
    if(code.type() == image.UPCE):
        return "UPCE"
    if(code.type() == image.ISBN10):
        return "ISBN10"
    if(code.type() == image.UPCA):
        return "UPCA"
    if(code.type() == image.EAN13):
        return "EAN13"
    if(code.type() == image.ISBN13):
        return "ISBN13"
    if(code.type() == image.I25):
        return "I25"
    if(code.type() == image.DATABAR):
        return "DATABAR"
    if(code.type() == image.DATABAR_EXP):
        return "DATABAR_EXP"
    if(code.type() == image.CODABAR):
        return "CODABAR"
    if(code.type() == image.CODE39):
        return "CODE39"
    if(code.type() == image.PDF417):
        return "PDF417"
    if(code.type() == image.CODE93):
        return "CODE93"
    if(code.type() == image.CODE128):
        return "CODE128"


#循环拍摄图像
while True:
    clock.tick()

    # 获取摄像头拍摄的图像
    img = sensor.snapshot()
    tmp_img = img

    #####################识别色块#####################
    # 找棕色快递箱色块
    blobs = tmp_img.find_blobs([thresholds[0]])

    if blobs:
        # 1. 过滤过小的色块（排除干扰，面积阈值根据实际场景调整）
        min_area = 5000  # 最小色块面积，单位：像素（可根据快递箱大小调整）
        valid_blobs = [b for b in blobs if b.area() > min_area]

        if valid_blobs:  # 确保过滤后还有有效色块
            # 2. 按面积排序，取最大的那个
            largest_blob = max(valid_blobs, key=lambda b: b.area())

            # 3. 只绘制最大色块的矩形和十字
            img.draw_rectangle(largest_blob[0:4], color=(0, 255, 0))  # 绿色框标记最大色块
            img.draw_cross(largest_blob[5], largest_blob[6], color=(0, 255, 0))

            # 记录快递箱中心坐标
            original_center_point = (largest_blob[5], largest_blob[6])

            # 显示最大色块的面积（可选，用于调试阈值）
            img.draw_string(0, 210, "Max Area: {}".format(largest_blob.area()), color=(255, 255, 255), scale=2)

            # 显示坐标
            img.draw_string(largest_blob[5] - 10, largest_blob[6] - 12, "({},{})".format(largest_blob[5], largest_blob[6]),color=(0, 255, 0), scale=1)

            # 找到快递箱
            original_center_point_flag = 1
        else:
            # 没有符合面积的色块
            img.draw_string(0, 210, "No valid blob", color=(255, 0, 0), scale=2)

    # 找快递单色块
    blobs = tmp_img.find_blobs(thresholds[2:])

    # 颜色名称对应列表，与阈值顺序一致
    color_map = {
        1: "red",
        2: "green",
        4: "blue"
    }

    warehouses = {
        "red": "B",
        "green": "A",
        "blue": "C"
    }

    if blobs:
        # 1. 过滤过小的色块（排除干扰，面积阈值根据实际场景调整）
        min_area = 3000  # 最小色块面积，单位：像素（可根据快递箱大小调整）
        valid_blobs = [b for b in blobs if b.area() > min_area]

        if valid_blobs:  # 确保过滤后还有有效色块
            # 2. 按面积排序，取最大的那个
            largest_blob = max(valid_blobs, key=lambda b: b.area())

            # 3. 只绘制最大色块的矩形和十字
            img.draw_rectangle(largest_blob[0:4], color=(255, 255, 255))  # 标记最大色块
            img.draw_cross(largest_blob[5], largest_blob[6], color=(255, 255, 255))

            # b[8] 是当前色块匹配的阈值索引（1:红色, 2:绿色, 4:蓝色）
            color_name = color_map.get(largest_blob[8], "unknown")

            # 记录快递种类
            warehouse = warehouses[color_name]

            # 在色块旁显示颜色名称
            img.draw_string(largest_blob[0], largest_blob[1]-10, color_name, color=(255,255,255), scale=2)

    # 找三个粉色定位点色块
    blobs = tmp_img.find_blobs([thresholds[1]])

    if blobs:
        # 1. 过滤过小的色块（排除干扰）
        min_area = 250  # 最小面积阈值，根据物体大小调整
        valid_blobs = [b for b in blobs if b.area() > min_area]

        if valid_blobs:
            # 2. 按面积从大到小排序
            valid_blobs.sort(key=lambda b: b.area(), reverse=True)

            # 3. 取前三个色块（如果不足三个则取实际数量）
            target_blobs = valid_blobs[:3]

            # 4. 绘制每个目标色块
            colors = [(0, 0, 255), (0, 0, 255), (0, 0, 255)]
            for i, b in enumerate(target_blobs):
                # 绘制矩形框和十字
                img.draw_rectangle(b[0:4], color=colors[i], thickness=2)
                img.draw_cross(b[5], b[6], color=colors[i], size=5)

                # 保存定位点坐标
                if original_center_point_flag:
                    anchor_point = (b[5], b[6])
                    anchor_points.append(anchor_point)

                # 显示序号和面积
                #img.draw_string(b.x(), b.y()-12, "#{}:{}".format(i+1, b.area()),color=colors[i], scale=1)

                # 显示坐标
                img.draw_string(b.x(), b.y()-12, "#{}:({},{})".format(i+1, b[5], b[6]),color=colors[i], scale=1)

            # 显示找到的色块数量
            img.draw_string(165, 210, "Found: {}/3".format(len(target_blobs)), color=(255, 255, 255), scale=2)

            # 成功找到3个色块
            if len(target_blobs) == 3:
                anchor_point_flag = 1
        else:
            # 没有符合面积的色块
            img.draw_string(165, 210, "No valid blobs", color=(255, 0, 0), scale=2)
    else:
        # 未检测到任何色块
        img.draw_string(165, 210, "No flags found", color=(255, 0, 0), scale=2)

    ####################识别条形码#####################
    codes = tmp_img.find_barcodes() #寻找图片中的条形码
    for code in codes:
        img.draw_rectangle(code.rect())
        img.draw_string(2,2, code.payload(), color=(255,255,255), scale=2)
        print_args = (barcode_name(code), code.payload(), (180 * code.rotation()) / math.pi, code.quality(), clock.fps())
        #print("Barcode %s, Payload \"%s\", rotation %f (degrees), quality %d, FPS %f" % print_args)

        #保存快递码
        if len(tracking_codes) >= 5:
            del tracking_codes[0]
        tracking_codes.append(code.payload())

    #快递码正确识别到的标志
    tracking_code_ready_flag = 1
    for i in range(0, 3):
        if len(tracking_codes) < 5 or tracking_codes[i] != tracking_codes[i + 1]:
            tracking_code_ready_flag = 0
            break


    ###################计算中心坐标####################
    #以下操作都必须要保证所有需求色块都被识别到了
    #计算快递箱相对中心坐标
    if original_center_point_flag and anchor_point_flag:
        if len(central_coordinates_list) >= 5:
            del central_coordinates_list[0]
        print("anchor_points {}".format(anchor_points))
        print("original_center_point {}".format(original_center_point))
        logical_coord = calculate_logical_coordinate(anchor_points, original_center_point)
        print("目标点的逻辑坐标: ({}, {})".format(logical_coord[0], logical_coord[1]))
        central_coordinates_list.append(logical_coord)

    #快递码正确识别，发送坐标
    if tracking_code_ready_flag and tracking_codes[0] != last_tracking_code and warehouse != 0 and original_center_point_flag and anchor_point_flag:
        #取5个坐标取平均值
        if len(central_coordinates_list) >= 5:
            central_coordinates_x = 0
            central_coordinates_y = 0
            for x, y in central_coordinates_list:
                central_coordinates_x += x
                central_coordinates_y += y
            central_coordinates = (central_coordinates_x / 5, central_coordinates_y / 5)

            #MQTT发布快递码和仓库
            MQTT_Send()
            #串口告诉机械臂坐标和仓库 @(x,y,color,area)*\n 机械臂x,y和我反着的
            uart_data = "@({},{},{})*\n".format(int(central_coordinates[1]), int(central_coordinates[0]), warehouse)
            uart.write(uart_data)

            print("tracking_code {}, central_coordinates ({}, {}), warehouse {}".format(tracking_codes[0], central_coordinates[0], central_coordinates[1], warehouse))
            print("uart_data {}".format(uart_data))
            #成功后保存为上一次快递码，避免重复发生
            last_tracking_code = tracking_codes[0];

    # 清空标志，方便下次读取判断
    anchor_points.clear()
    anchor_point_flag = 0
    original_center_point_flag = 0
    warehouse = 0


    lcd.display(img)     #LCD显示图片
    #print("FPS %f" % clock.fps())   #打印FPS
